/*
 * @Author: _oufen
 * @Date: 2023-07-11 20:23:19
 * @LastEditTime: 2023-08-05 04:21:45
 * @Description:  主要控制代码
 */
/**
 *************************************************************************************************************************
 * @file    control.c
 * @author  oufen
 * @version V1.0
 * @date    2023-07-10
 * @brief   Control模块.c文件配置
 *************************************************************************************************************************
 * @attention
 *
 *
 *************************************************************************************************************************
 */

/* Includes -------------------------------------------------------------------------------------------------------------*/
#include "control.h"

/* 定义 -----------------------------------------------------------------------------------------------------------------*/
Param_InitTypedef Param;
Flag_InitTypedef Flag;

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 求出直线坐标  两点
// 并且求  两点之间直线上的点   上的坐标

// 求出dx
float get_dx(float x1, float x2, int n)
{
    return (x2 - x1) / n;
}

// 求出dy
float get_dy(float y1, float y2, int n)
{
    return (y2 - y1) / n;
}

// 给出 x1,x2  第几个坐标 两点间横坐标之差
float get_x(float x1, float x2, int N)
{
    // return x1 + (x2 - x1) / N;

    return x1 + N * get_dx(x1, x2, N);
}

// 给出 x1,x2  第几个坐标 两点间横坐标之差
float get_y(float y1, float y2, int N)
{
    // return x1 + (x2 - x1) / N;

    return y1 + N * get_dy(y1, y2, N);
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/**
 * @brief 主控制函数
 * @param  无
 * @retval  无
 **/
uint8_t maincon_flag, maincon_flag2;
uint8_t my_i1;

float dx, dy;
void mainControl(void)
{
    static int flag = -1;       // 步骤运行
    static uint32_t myTime = 0; // 计时
    switch (flag)
    {
    case -1: // 从中心点去 直接目标
             /* code */
        if (Zuoshang_x != 0 && Zuoshang_y != 0 && you_shang_x != 0 && you_shang_y != 0 && youxia_x != 0 && youxia_y != 0 && zuoxia_x != 0 && zuoxia_y != 0)
        {
            if (Red_X < you_shang_x) // 激光在矩形左边  往右移
            {
                x_pwm -= 1;
                if (Red_X >= you_shang_x - 1 && Red_X <= you_shang_x + 1)
                {
                    maincon_flag = 1;
                    // my_task = 1;
                }
            }
            else if (Red_X > you_shang_x) // 激光在矩形右边  往左移
            {
                x_pwm += 1;
                if (Red_X >= you_shang_x - 1 && Red_X <= you_shang_x + 1)
                {
                    maincon_flag = 1;
                    // my_task = 1;
                }
            }
            if (Red_Y > you_shang_y) // 激光在矩形下方   往上移
            {
                y_pwm -= 1; // pwm减 往上移
                if (Red_Y >= you_shang_y - 1 && Red_Y <= you_shang_y + 1)
                {
                    maincon_flag2 = 1;
                    // my_task = 1;
                }
            }
            else if (Red_Y < you_shang_y) // 激光在矩形上方  往下移
            {
                y_pwm += 1; // pwm减 往下移
                if (Red_Y >= you_shang_y - 1 && Red_Y <= you_shang_y + 1)
                {
                    maincon_flag2 = 1;
                    // my_task = 1;
                }
            }
            if (maincon_flag == 1 && maincon_flag2 == 1)
            {
                // flag = 0;
                flag = 99;
            }
        }

        break;
    case 99:
        pid_servo_X.target_val = youxia_x; // 这个就是这条直线 上的所有点
        pid_servo_Y.target_val = youxia_y;

        x_pwm -= Servo_X_pid_control();
        y_pwm += Servo_Y_pid_control();

        if ((Red_X >= pid_servo_X.target_val - 2) && (Red_X <= pid_servo_X.target_val + 2) && (Red_Y >= pid_servo_Y.target_val - 2) && (Red_Y <= pid_servo_Y.target_val + 2)) // 限幅
        {
            flag = 1;
        }

        break;
    case 0: // 知道目标点和实际点坐标

        //        for (my_i1 = 0; my_i1 <= 20; my_i1++)
        //        {
        //            dx = get_dx(youxia_x, you_shang_x, my_i1);
        //            dy = get_dy(youxia_y, you_shang_y, my_i1);
        //            pid_servo_X.target_val = you_shang_x + my_i1 * dx; // 这个就是这条直线 上的所有点
        //            pid_servo_Y.target_val = you_shang_y + my_i1 * dy;

        //            x_pwm -= Servo_X_pid_control();
        //            y_pwm += Servo_Y_pid_control();

        //            if ((Red_X >= you_shang_x - 1 && Red_X <= you_shang_x + 1) || ((Red_Y >= you_shang_y - 1 && Red_Y <= you_shang_y + 1))) // 限幅
        //            {
        //                flag = 1;
        //            }
        //        }

        pid_servo_X.target_val = (youxia_x + you_shang_x) / 2.0; // 这个就是这条直线 上的所有点
        pid_servo_Y.target_val = (youxia_y + you_shang_y) / 2.0;

        x_pwm -= Servo_X_pid_control();
        y_pwm += Servo_Y_pid_control();

        if ((Red_X >= pid_servo_X.target_val - 2) && (Red_X <= pid_servo_X.target_val + 2) && (Red_Y >= pid_servo_Y.target_val - 2) && (Red_Y <= pid_servo_Y.target_val + 2)) // 限幅
        {
            // flag = 1;
            flag = 99;
        }

        break;
    case 1:
        pid_servo_X.target_val = zuoxia_x; // 这个就是这条直线 上的所有点
        pid_servo_Y.target_val = zuoxia_y;

        x_pwm -= Servo_X_pid_control();
        y_pwm += Servo_Y_pid_control();

        if ((Red_X >= zuoxia_x - 2 && Red_X <= zuoxia_x + 2) && ((Red_Y >= zuoxia_y - 2 && Red_Y <= zuoxia_y + 2))) // 限幅
        {
            flag = 2;
        }

        break;
    case 2:
        pid_servo_X.target_val = Zuoshang_x; // 这个就是这条直线 上的所有点
        pid_servo_Y.target_val = Zuoshang_y;

        x_pwm -= Servo_X_pid_control();
        y_pwm += Servo_Y_pid_control();

        if ((Red_X >= Zuoshang_x - 2 && Red_X <= Zuoshang_x + 2) && ((Red_Y >= Zuoshang_y - 2 && Red_Y <= Zuoshang_y + 2))) // 限幅
        {
            flag = 3;
        }
        break;
    case 3:
        pid_servo_X.target_val = you_shang_x; // 这个就是这条直线 上的所有点
        pid_servo_Y.target_val = you_shang_y;

        x_pwm -= Servo_X_pid_control();
        y_pwm += Servo_Y_pid_control();

        if ((Red_X >= you_shang_x - 2 && Red_X <= you_shang_x + 2) && ((Red_Y >= you_shang_y - 2 && Red_Y <= you_shang_y + 2))) // 限幅
        {
            flag = 4;
        }
        break;
    case 4:
        break;
    default:
        break;
    }
}

/**
 * @brief 	TIM6中断   10ms
 * @param 	无
 * @retval  无
 */
float speed1,
    speed2;

float X_pwm,
    Y_pwm;
void TIM6_IRQHandler(void) // 10ms
{
    if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)
    {
        LED_RED = !LED_RED; // 测试程序是否卡死
        TIM_ClearITPendingBit(TIM6, TIM_IT_Update);

        // task4_control();

        mainControl();

        // pid_servo_X.target_val = you_shang_x; // 这个就是这条直线 上的所有点
        // pid_servo_Y.target_val = you_shang_y;

        // x_pwm -= Servo_X_pid_control();
        // y_pwm += Servo_Y_pid_control();

        //  if(Red_X)
    }
}
/*****************************************************END OF FILE*********************************************************/
